/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : app_freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "bsp.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
static TaskHandle_t xHandleTaskStart = NULL;
static TaskHandle_t xHandleTaskUserIF = NULL;
static TaskHandle_t xHandleTaskUartShell = NULL;
static TaskHandle_t xHandleTaskUartRx = NULL;  // 新增串口接收任务句柄

SemaphoreHandle_t xTxSemaphore;  // 用于发送完成的信号量
SemaphoreHandle_t xUartMutex;    // 用于保护串口资源的互斥锁
SemaphoreHandle_t xFocMutex;    // 用于保护FOC参数资源的互斥锁
SemaphoreHandle_t xDataMutex;  	//Vofe保护打印数据互斥锁

// 串口接收相关变量
SemaphoreHandle_t xUartRxSemaphore = NULL;
QueueHandle_t xUartFrameQueue = NULL;

/* USER CODE END Variables */
/* Definitions for defaultTask */
osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = {
  .name = "defaultTask",
  .priority = (osPriority_t) osPriorityNormal,
  .stack_size = 128 * 4
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */
static void AppTaskUartRx(void *argument);

static void AppTaskStart(void *argument);
static void AppTaskCreate(void);
static void AppTaskUserIF(void *argument);
static void AppTaskUartShell(void *argument);
void print_vofa_frame(void);

/* USER CODE END FunctionPrototypes */

void StartDefaultTask(void *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */
  // 初始化串口DMA接收
  HAL_UART_Receive_DMA(&huart3, uart_rx_buffer, UART_RX_BUFFER_SIZE);
  __HAL_UART_ENABLE_IT(&huart3, UART_IT_IDLE);  // 使能空闲中断
	
  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
	xUartMutex = xSemaphoreCreateMutex();
	xFocMutex = xSemaphoreCreateMutex();
	xDataMutex = xSemaphoreCreateMutex();
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
	xTxSemaphore = xSemaphoreCreateBinary();
	xUartRxSemaphore = xSemaphoreCreateBinary();  // 创建串口接收信号量
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
	xUartFrameQueue = xQueueCreate(10, sizeof(UartFrame));  // 创建帧队列
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* creation of defaultTask */
  defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
	xTaskCreate(  AppTaskStart,         /* 任务函数  */
								"AppTaskStart",       /* 任务名    */
								512,                 	/* 任务栈大小，单位word，也就是4字节 */
								NULL,                 /* 任务参数  */
								osPriorityNormal6,    /* 任务优先级*/
								&xHandleTaskStart );  /* 任务句柄  */
								
  // 创建串口接收任务
  xTaskCreate(  AppTaskUartRx,        /* 任务函数  */
                "AppTaskUartRx",      /* 任务名    */
                512,                  /* 任务栈大小，单位word，也就是4字节 */
                NULL,                 /* 任务参数  */
                osPriorityNormal3,    /* 任务优先级（低于电机控制）*/
                &xHandleTaskUartRx ); /* 任务句柄  */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_EVENTS */
  /* add events, ... */
  /* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_StartDefaultTask */
/**
  * @brief  Function implementing the defaultTask thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument)
{
  /* USER CODE BEGIN StartDefaultTask */
  /* Infinite loop */
  for(;;)
  {
    osDelay(1);
  }
  /* USER CODE END StartDefaultTask */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */


/*
*********************************************************************************************************
*	函 数 名: AppTaskUartRx
*	功能说明: 串口接收任务，处理DMA接收的不定长数据
*	形    参: argument - 任务参数
*	返 回 值: 无
* 优 先 级: osPriorityNormal3
*********************************************************************************************************
*/
static void AppTaskUartRx(void *argument)
{
	xSemaphoreGive(xTxSemaphore);
//  UartFrame frame;
  uint16_t data_length;
  
  shellPrint(&shell, "[AppTaskCreate]:AppTaskUartRx task create success\r\n");
  
  while(1)
  {
    // 等待信号量（由空闲中断释放）
    if(xSemaphoreTake(xUartRxSemaphore, portMAX_DELAY) == pdTRUE)
    {
      // 计算接收到的数据长度
      if(dma_current_pos >= dma_last_pos)
      {
        data_length = dma_current_pos - dma_last_pos;
      }
      else
      {
        data_length = UART_RX_BUFFER_SIZE - dma_last_pos + dma_current_pos;
      }
      
      // 处理接收到的数据
      if(data_length > 0)
      {
        ProcessUartData(dma_last_pos, data_length);
      }
      
      dma_last_pos = dma_current_pos;
    }
    
    // 短暂延时，让出CPU
    vTaskDelay(1);
  }
}

/*
*********************************************************************************************************
*	函 数 名: AppTaskUserIF
*	功能说明: 按键消息处理
*	形    参: 无
*	返 回 值: 无
* 优 先 级: osPriorityNormal4
*********************************************************************************************************
*/
static void AppTaskUserIF(void *argument)
{
	uint8_t ucKeyCode;
	/* 初始化任务 */
	
	while(1)
	{
		ucKeyCode = bsp_GetKey();
		switch (ucKeyCode)
		{
			case KEY_DOWN_K1:		/* K1键按下 */
				shellPrint(&shell, "KEY_DOWN_K1\r\n");
				// 获取互斥锁，保护 FOC参数 资源
				xSemaphoreTake(xFocMutex, portMAX_DELAY);
				switch (UsrConfig.control_mode)
				{
					case CONTROL_MODE_TORQUE_RAMP:
						Foc.target_i_q -= 0.1f;
						break;
					case CONTROL_MODE_VELOCITY_RAMP:
						Foc.target_velocity -= 5.0f;
						break;
					case CONTROL_MODE_POSITION_RAMP:
						Foc.target_position -= 5.0f;
						break;
					case CONTROL_MODE_POSITION_PROFILE:
						Foc.target_position -= 5.0f;
						break;
					default:
						break;
				}
				// 释放互斥锁
				xSemaphoreGive(xFocMutex);
				break;
				
			case KEY_DOWN_K2:		/* K2键按下 */
				shellPrint(&shell, "KEY_DOWN_K3\r\n");
				// 获取互斥锁，保护 FOC参数 资源
				xSemaphoreTake(xFocMutex, portMAX_DELAY);
				switch (UsrConfig.control_mode)
				{
					case CONTROL_MODE_TORQUE_RAMP: //控制电机的输出扭矩（转矩） 给定一个目标扭矩，控制器会通过斜坡函数（ramp）逐渐调整实际扭矩至目标值，以避免扭矩突变，从而减少机械应力或冲击。适用于需要精确控制扭矩的应用，如张力控制、压力控制等。
						Foc.target_i_q += 0.1f;
						break;
					case CONTROL_MODE_VELOCITY_RAMP://控制电机的转速（角速度） 通过斜坡函数逐渐调整实际速度至目标值，以实现平滑的加速或减速过程，避免速度突变。	适用于需要平滑速度控制的应用，如传送带、风机、泵等。
						Foc.target_velocity += 5.0f;
						break;
					case CONTROL_MODE_POSITION_RAMP://控制电机的位置（角度或线性位置）。给定目标位置，控制器会通过斜坡函数生成一个平滑的速度曲线（通常是梯形或S形速度曲线）来达到目标位置，从而避免位置突变和机械冲击。适用于需要平滑移动到目标位置的应用，如机械臂、数控机床等。
						Foc.target_position += 5.0f;
						break;
					case CONTROL_MODE_POSITION_PROFILE://控制位置，但它通常使用更复杂的运动规划（profiled motion），如梯形速度曲线、S形曲线（S-curve）等。与位置斜坡模式相比，位置轮廓模式通常允许用户设置更详细的运动参数，如加速度、减速度、最大速度等，以实现更精确和平滑的运动控制。适用于对运动轨迹要求较高的应用，如机器人、精密定位系统等。
						Foc.target_position += 5.0f;
						break;
					default:
						break;
				}
				// 释放互斥锁
				xSemaphoreGive(xFocMutex);
				break;

			case KEY_DOWN_K3:		/* K3键按下 */
				/* 急停 */
				shellPrint(&shell, "KEY_DOWN_K3\r\n");
				MCT_set_state(M_ERR); /* 设置为错误状态 */
				break;
				
			case KEY_DOWN_K4: 	/* K4键按下 */
				shellPrint(&shell, "KEY_DOWN_K4\r\n");
				// 获取互斥锁，保护 FOC参数 资源
				xSemaphoreTake(xFocMutex, portMAX_DELAY);
				if (UsrConfig.control_mode < 4)
				{
					UsrConfig.control_mode++;
				}
				else
				{
					UsrConfig.control_mode = 1;
				}
				// 释放互斥锁
				xSemaphoreGive(xFocMutex);
				break;
				
			case SYS_DOWN_K1K2:
				shellPrint(&shell, "SYS_DOWN_K1K2\r\n");
				SPI3_Ctrl = !SPI3_Ctrl; /* 切换SPI3控制对象 */
				break;
			default:
				break;
		}
		/* 需要周期性处理的程序 */
		vTaskDelay(20);
	}
}
/*
*********************************************************************************************************
*	函 数 名: AppTaskUartShell
*	功能说明: 串口shell任务，用于接收串口数据，并执行相关命令。
*	形    参: 无
*	返 回 值: 无
* 优 先 级: osPriorityNormal5
*********************************************************************************************************
*/
static void AppTaskUartShell(void *argument)
{
	UartFrame frame;
	/* 初始化任务 */
	while(1)
	{
		/* 需要周期性处理的程序 */
		shellTask(&shell);
//		print_vofa_frame();
    // 检查并处理接收到的帧
    if(xQueueReceive(xUartFrameQueue, &frame, 0) == pdTRUE)
    {
      ProcessUartFrame(&frame);
    }
		vTaskDelay(10);
	}
}
/*
*********************************************************************************************************
*	函 数 名: AppTaskStart
*	功能说明: 启动任务，这里用作BSP驱动包处理。
*	形    参: 无
*	返 回 值: 无
* 优 先 级: osPriorityNormal6  
*********************************************************************************************************
*/
static void AppTaskStart(void *argument)
{
	uint8_t  ucCount = 0;
	uint32_t  ucCount1 = 0;
	/* 初始化外设 */
	
	/* 初始化组件 */
	bsp_InitKey();		 				/* 按键初始化 */
	User_Shell_Init(); 					/* letter-shell初始化 */
	MyFoc_init();						/* FOC初始化 */
	
	
	AppTaskCreate();			/* 创建任务 */
	
	while(1)
	{
		/* 需要周期性处理的程序 */
		
		
		/* 1ms 处理 */
		bsp_RunPer1ms();
		/* 10ms 处理 */
		if(++ucCount == 10)
		{
			ucCount = 0;
			bsp_RunPer10ms();
		}
		/* 1000ms 处理 */
		if(++ucCount1 == 1000)
		{
			ucCount1 = 0;
			bsp_LedToggle(1);
		}
		vTaskDelay(1);
	}
}

/*
*********************************************************************************************************
*	函 数 名: AppTaskCreate
*	功能说明: 创建应用任务
*	形    参：无
*	返 回 值: 无
*********************************************************************************************************
*/
static void AppTaskCreate(void)
{
	BaseType_t status;

	status = xTaskCreate(  AppTaskUserIF,  			 	 /* 任务函数  */
												"AppTaskUserIF", 			 	 /* 任务名    */
												256,                 		 /* 任务栈大小，单位word，也就是4字节 */
												NULL,                 	 /* 任务参数  */
												osPriorityNormal4,    	 /* 任务优先级*/
												&xHandleTaskUserIF );    /* 任务句柄  */
	if ( pdPASS == status)
		shellPrint(&shell, "[AppTaskCreate]:AppTaskUserIF task create success\r\n");
	else
		shellPrint(&shell, "[AppTaskCreate]:AppTaskUserIF task create failed\r\n");

	
	status = xTaskCreate(  AppTaskUartShell,  			 /* 任务函数  */
												"AppTaskUartShell", 			 /* 任务名    */
												512,                 			 /* 任务栈大小，单位word，也就是4字节 */
												NULL,                 		 /* 任务参数  */
												osPriorityNormal5,    		 /* 任务优先级*/
												&xHandleTaskUartShell );   /* 任务句柄  */
	if ( pdPASS == status)
		shellPrint(&shell, "[AppTaskCreate]:AppTaskUartShell task create success\r\n");
	else
		shellPrint(&shell, "[AppTaskCreate]:AppTaskUartShell task create failed\r\n");
	
	shellPrint(&shell, "*************************************************************\r\n");
}


/*
*********************************************************************************************************
*	函 数 名: HAL_ADCEx_InjectedConvCpltCallback
*	功能说明: ADC注入转换完成回调函数
*	形    参：hadc：ADC句柄
*	返 回 值: 无
*********************************************************************************************************
*/
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *hadc)
{
	/* Calc ADC offset */
	static int adc_sum_a = 0;
	static int adc_sum_b = 0;
	static int adc_sum_c = 0;
	static uint8_t isCalcAdcOffsetOvered = FALSE;
	const int measCnt = 64;
	static int measCntCopy = measCnt;
	if (hadc->Instance == ADC1 && !isCalcAdcOffsetOvered)
	{
		adc_sum_a += hadc1.Instance->JDR1;
		adc_sum_b += hadc1.Instance->JDR2;
		adc_sum_c += hadc1.Instance->JDR3;

		if (--measCntCopy <= 0)
		{
			phase_a_adc_offset = adc_sum_a / measCnt;
			phase_b_adc_offset = adc_sum_b / measCnt;
			phase_c_adc_offset = adc_sum_c / measCnt;

			isCalcAdcOffsetOvered = TRUE;
		}
	}
	if (hadc->Instance == ADC1 && isCalcAdcOffsetOvered)
	{
		
    MCT_high_frequency_task();
		
		vofaFrame.fdata[0] = Foc.i_a;
		vofaFrame.fdata[1] = Foc.i_b;
		vofaFrame.fdata[2] = Foc.i_c;
		vofaFrame.fdata[3] = Foc.i_d_filt;
		vofaFrame.fdata[4] = Foc.i_q_filt;
		vofaFrame.fdata[5] = Foc.target_position;
		vofaFrame.fdata[6] = Foc.position;
		vofaFrame.fdata[7] = Foc.target_velocity;
		vofaFrame.fdata[8] = Foc.velocity_filt;
//		vofa_print();
	}
}

/*
*********************************************************************************************************
*	函 数 名: PrintfLogo
*	功能说明: 打印例程名称和例程发布日期, 接上串口线后，打开PC机的超级终端软件可以观察结果
*	形    参: 无
*	返 回 值: 无
*********************************************************************************************************
*/
void PrintfLogo(void)
{
	
	/* 检测CPU ID */
	uint32_t CPU_Sn0, CPU_Sn1, CPU_Sn2;
	
	CPU_Sn0 = HAL_GetUIDw0();
	CPU_Sn1 = HAL_GetUIDw1();
	CPU_Sn2 = HAL_GetUIDw2();
	
	shellPrint(&shell, "*************************************************************\r\n");
	shellPrint(&shell, "    __________  ______   ______________ \r\n");
	shellPrint(&shell, "   / ____/ __ \\/ ____/  / ____/_  __/ / \r\n");
	shellPrint(&shell, "  / /_  / / / / /      / /     / / / /  \r\n");
	shellPrint(&shell, " / __/ / /_/ / /___   / /___  / / / /___\r\n");
	shellPrint(&shell, "/_/    \\____/\\____/   \\____/ /_/ /_____/\r\n");
	shellPrint(&shell, "                                        \r\n");
	shellPrint(&shell, "Build:       "__DATE__" "__TIME__"\r\n");
	shellPrint(&shell, "Version:     "SOFTWARE_VERSION"\r\n");
	shellPrint(&shell, "Copyright:   (C) 2025 astronG\r\n");
	shellPrint(&shell, "-------------------------------------------------------------\r\n");
	shellPrint(&shell, "CPU: STM32G474RET6, SYSCLK: %dMHz, RAM: 128KB, ROM: 512KB\r\n", SystemCoreClock / 1000000);
	shellPrint(&shell, "UID: %08X %08X %08X\r\n", CPU_Sn2, CPU_Sn1, CPU_Sn0);
	shellPrint(&shell, "STM32G4xx_HAL_Driver:STM32Cube_FW_G4 V1.6.1\r\n");
//	shellPrint(&shell, "Current version of FOC Algorithm: %s\r\n", MYFOC_SOFTWARE_VERSION);
  shellPrint(&shell, "*************************************************************\r\n");
}

void print_vofa_frame(void)
{
    // 定义 FireWater 协议的数据结构
    typedef struct {
        float i_a;
        float i_b;
        float i_c;
        float i_d_filt;
        float i_q_filt;
        float position;
        float velocity;
        float target_position;
        float target_velocity;
        uint8_t tail[4]; // FireWater 协议帧尾
    } vofa_data_t;
    
    static vofa_data_t vofa_data = {0};
    
    // 设置帧尾（FireWater 协议特定的结束符）
    vofa_data.tail[0] = 0x00;
    vofa_data.tail[1] = 0x00;
    vofa_data.tail[2] = 0x80;
    vofa_data.tail[3] = 0x7F;
    
    // 获取互斥锁保护数据访问
    if(xSemaphoreTake(xDataMutex, pdMS_TO_TICKS(10))) {
        // 填充数据（注意：这里使用临时变量避免多次访问）
        float temp[9];
        for(int i = 0; i < 9; i++) {
            temp[i] = vofaFrame.fdata[i];
        }
        
        vofa_data.i_a = temp[0];
        vofa_data.i_b = temp[1];
        vofa_data.i_c = temp[2];
        vofa_data.i_d_filt = temp[3];
        vofa_data.i_q_filt = temp[4];
        vofa_data.target_position = temp[5];
        vofa_data.position = temp[6];
        vofa_data.target_velocity = temp[7];
        vofa_data.velocity = temp[8];
        
        xSemaphoreGive(xDataMutex);
        
        // 发送二进制数据
        HAL_UART_Transmit_DMA(&huart3, (uint8_t*)&vofa_data, sizeof(vofa_data_t));
    }
}

/* USER CODE END Application */

